// action: /follow_waypoints [nav2_msgs/action/FollowWaypoints]

/*
#goal definition
geometry_msgs/PoseStamped[] poses
    std_msgs/Header header
        builtin_interfaces/Time stamp
            int32 sec
            uint32 nanosec
        string frame_id
    Pose pose
        Point position
            float64 x
            float64 y
            float64 z
        Quaternion orientation
            float64 x 0
            float64 y 0
            float64 z 0
            float64 w 1
---
#result definition
int32[] missed_waypoints
---
#feedback definition
uint32 current_waypoint
 */

#include "rclcpp/rclcpp.hpp"
#include "nav2_msgs/action/follow_waypoints.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

using nav2_msgs::action::FollowWaypoints;
using std::placeholders::_1;
using std::placeholders::_2;

class WayPoint_Follower : public rclcpp::Node
{
private:
    rclcpp_action::Client<FollowWaypoints>::SharedPtr client_;

public:
    WayPoint_Follower() : Node("waypoint_follower_node")
    {
        client_ = rclcpp_action::create_client<FollowWaypoints>(this, "/follow_waypoints");
    }
    void send_goal()
    {
        if (!client_->wait_for_action_server())
        {
            RCLCPP_INFO(this->get_logger(), "连接服务器失败！");
        }

        auto goal = FollowWaypoints::Goal();
        std::vector<geometry_msgs::msg::PoseStamped> waypoints;
        auto current_time = this->get_clock()->now();

        geometry_msgs::msg::PoseStamped waypoint1;
        waypoint1.header.frame_id = "map";
        waypoint1.header.stamp = current_time;
        waypoint1.pose.position.x = 1.0;
        waypoint1.pose.position.y = 1.0;
        waypoint1.pose.position.z = 0.0;
        waypoint1.pose.orientation.w = 1.0;
        waypoints.push_back(waypoint1);

        geometry_msgs::msg::PoseStamped waypoint2;
        waypoint2.header.frame_id = "map";
        waypoint2.header.stamp = current_time;
        waypoint2.pose.position.x = 1.0;
        waypoint2.pose.position.y = -1.0;
        waypoint2.pose.position.z = 0.0;
        waypoint2.pose.orientation.w = 1.0;
        waypoints.push_back(waypoint2);

        geometry_msgs::msg::PoseStamped waypoint3;
        waypoint3.header.frame_id = "map";
        waypoint3.header.stamp = current_time;
        waypoint3.pose.position.x = -1.0;
        waypoint3.pose.position.y = -1.0;
        waypoint3.pose.position.z = 0.0;
        waypoint3.pose.orientation.w = 1.0;
        waypoints.push_back(waypoint3);

        goal.set__poses(waypoints);

        rclcpp_action::Client<FollowWaypoints>::SendGoalOptions options;
        options.feedback_callback = std::bind(&WayPoint_Follower::FeedBack_CallBack, this, _1, _2);
        options.goal_response_callback = std::bind(&WayPoint_Follower::Goal_Response_CallBack, this, _1);
        options.result_callback = std::bind(&WayPoint_Follower::Result_CallBack, this, _1);
        client_->async_send_goal(goal, options);
    }
    void FeedBack_CallBack(rclcpp_action::ClientGoalHandle<FollowWaypoints>::SharedPtr goal_handle, const std::shared_ptr<const FollowWaypoints::Feedback> feedback)
    {
        (void)goal_handle;
        RCLCPP_INFO(this->get_logger(), "current_waypoint:%d", feedback->current_waypoint);
    }
    void Goal_Response_CallBack(rclcpp_action::ClientGoalHandle<FollowWaypoints>::SharedPtr goal_handle)
    {
        if (!goal_handle)
        {
            RCLCPP_INFO(this->get_logger(), "服务器拒绝！");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "服务器接收！");
        }
    }
    void Result_CallBack(const rclcpp_action::ClientGoalHandle<FollowWaypoints>::WrappedResult &result)
    {
        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(this->get_logger(), "路点导航成功！");
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_INFO(this->get_logger(), "路点导航功能阻塞！");
            break;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_INFO(this->get_logger(), "路点导航功能终止！");
            break;
        default:
            RCLCPP_INFO(this->get_logger(), "路点导航功能未知异常！");
            break;
        }
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<WayPoint_Follower>();
    node->send_goal();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
